#include <ros/ros.h>
#include <grid_map_ros/grid_map_ros.hpp>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/transform_listener.h>

#include "stair_ctrl/MotionPlan.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <algorithm>
#include <vector>
#include <iterator>
#include <cmath>

#include "Geometry.hpp"

class mapManager
{
private:
    // grid_map::GridMap map_;
    std::shared_ptr<grid_map::GridMap> map_, map_temp_;
    ros::Subscriber grid_map_sub_;
    ros::Subscriber plan_sub_;
    ros::Publisher plan_pub_;
    ros::Publisher log_pub_;
    ros::CallbackQueue map_queue_;

    tf2_ros::Buffer *tfBuffer;
    tf2_ros::TransformListener *tfListener;

    std::string map_frame_;
    std::string base_frame_;

    double bodyHeight_ = 0.3;
    bool accurateStanceFoot_ = true;

    std::string elevation_layer_;
    std::string dx_layer_;
    std::string dy_layer_;
    std::string wz_layer_;
    std::string ax_layer_;
    std::string ay_layer_;
    bool stair_flag_ = false;
    bool ax_flag_ = false;
    bool ay_flag_ = false;
    bool normalized_gradient_ = true;

    bool sample_based_adjustment_ = false;

    bool ready = false;

public:
    mapManager(ros::NodeHandle *nh, ros::NodeHandle *nh_map)
    {
        auto hints = ros::TransportHints().tcpNoDelay();
        std::string map_topic, req_topic;

        nh->param<std::string>("map_topic", map_topic, "/elevation_mapping/elevation_map");
        nh->param<std::string>("request_topic", req_topic, "/stair_ctrl/vision_request");

        nh->param<std::string>("map_frame", map_frame_, "odom");
        nh->param<std::string>("base_frame", base_frame_, "com");
        nh->param<std::string>("elevation_layer", elevation_layer_, "elevation");
        nh->param<std::string>("dx_layer", dx_layer_, "dx");
        nh->param<std::string>("dy_layer", dy_layer_, "dy");
        nh->param<std::string>("wz_layer", wz_layer_, "wz");
        nh->param<std::string>("ax_layer", ax_layer_, "ax");
        nh->param<std::string>("ay_layer", ay_layer_, "ay");

        nh->param<bool>("normalized_gradient", normalized_gradient_, true);

        tfBuffer = new tf2_ros::Buffer();
        tfListener = new tf2_ros::TransformListener(*tfBuffer);

        sample_based_adjustment_ = ax_layer_ == "sample";

        // ros::SubscribeOptions map_ops = ros::SubscribeOptions::initByFullCallbackType(
        // ros::SubscribeOptions map_ops = getSubscribeOptions(
        //     map_topic,                      // topic name
        //     1,                              // queue length
        //     &mapManager::grid_map_callback, // callback
        //     this,
        //     &map_queue_);
        // // ros::SubscribeOptions plan_ops = ros::SubscribeOptions::initByFullCallbackType(
        // ros::SubscribeOptions plan_ops = getSubscribeOptions(
        //     req_topic,                            // topic name
        //     1,                                    // queue length
        //     &mapManager::vision_request_callback, // callback
        //     this,
        //     &plan_queue_ // pointer to callback queue object
        // );
        // ros::SubscribeOptions map_ops =
        //     ros::SubscribeOptions::create<grid_map_msgs::GridMap>(
        //         map_topic, // topic name
        //         1,              // queue length
        //         &mapManager::grid_map_callback,  // callback
        //         ros::VoidPtr(), // tracked object, we don't need one thus NULL
        //         &map_queue_    // pointer to callback queue object
        //     );
        // ros::SubscribeOptions plan_ops =
        //     ros::SubscribeOptions::create<stair_ctrl::MotionPlan>(
        //         req_topic, // topic name
        //         1,              // queue length
        //         &mapManager::vision_request_callback,  // callback
        //         ros::VoidPtr(), // tracked object, we don't need one thus NULL
        //         &plan_queue_    // pointer to callback queue object
        //     );
        // map_ops.tracked_object = shared_from_this();
        // map_ops.tracked_object = shared_from_this();
        // map_ops.transport_hints = hints;
        // plan_ops.transport_hints = hints;
        // grid_map_sub_ = nh->subscribe(map_ops);
        // plan_sub_ = nh->subscribe(plan_ops);

        nh_map->setCallbackQueue(&map_queue_);
        grid_map_sub_ = nh_map->subscribe(map_topic, 10, &mapManager::grid_map_callback, this, hints);
        
        plan_sub_ = nh->subscribe(req_topic, 10, &mapManager::vision_request_callback, this, hints);
        plan_pub_ = nh->advertise<stair_ctrl::MotionPlan>("step_plan", 100);
        log_pub_ = nh->advertise<stair_ctrl::MotionPlan>("log", 100);

        map_ = std::make_shared<grid_map::GridMap>();
        map_temp_ = std::make_shared<grid_map::GridMap>();
    }

    mapManager(const mapManager &) = delete;

    ~mapManager() = default;

    void grid_map_callback(const grid_map_msgs::GridMap &msg)
    {
        ROS_DEBUG("get map");
        grid_map::GridMapRosConverter::fromMessage(msg, *map_temp_);
        map_.swap(map_temp_);
        auto layers = map_->getLayers();
        stair_flag_ = std::find(layers.begin(), layers.end(), wz_layer_) != layers.end();
        ax_flag_ = std::find(layers.begin(), layers.end(), ax_layer_) != layers.end();
        ay_flag_ = std::find(layers.begin(), layers.end(), ay_layer_) != layers.end();

        ready = true;
        ROS_DEBUG("map saved");
    }

    void vision_request_callback(const stair_ctrl::MotionPlan::ConstPtr &msg)
    {
        if (!ready)
        {
            ROS_WARN("No map!");
            return;
        }

        Eigen::Matrix<float, 12, 1> foot_pos;
        Eigen::Matrix<double, 12, 1> endEffectorOffset;
        Eigen::Matrix<double, 12, 1> endEffectorW;
        Eigen::Matrix<float, 3, 1> vel_des;
        foot_pos.setZero();
        vel_des.setZero();
        endEffectorOffset.setZero();
        endEffectorW.setZero();

        Eigen::Matrix<float, 12, 1> endEffectorRefFloat;
        Eigen::Matrix<float, 3, 1> pos_des;
        endEffectorRefFloat.setZero();
        pos_des.setZero();

        for (size_t i = 0; i < 12; ++i)
        {
            foot_pos(i) = msg->foot_pos.at(i);
        }
        for (size_t i = 0; i < 3; ++i)
        {
            vel_des(i) = msg->vec3.at(i);
        }
        float phase = msg->phase;

        geometry_msgs::TransformStamped transformStamped;
        transformStamped = tfBuffer->lookupTransform(map_frame_, base_frame_, ros::Time(0));

        ROS_DEBUG_STREAM("[TF] (x, y, z) = (" << transformStamped.transform.translation.x
                                              << ", " << transformStamped.transform.translation.y
                                              << ", " << transformStamped.transform.translation.z
                                              << "); (w, x, y, z) = (" << transformStamped.transform.rotation.w
                                              << ", " << transformStamped.transform.rotation.x
                                              << ", " << transformStamped.transform.rotation.y
                                              << ", " << transformStamped.transform.rotation.z << ")");

        Eigen::Quaternion<double> quat(transformStamped.transform.rotation.w,
                                       transformStamped.transform.rotation.x,
                                       transformStamped.transform.rotation.y,
                                       transformStamped.transform.rotation.z);
        auto rotmat = quat.toRotationMatrix();
        Eigen::Matrix<double, 3, 1> vel_des_lin(static_cast<double>(vel_des(0)), static_cast<double>(vel_des(1)), 0.0);
        // auto vel_des_lin = vel_des.cast<double>();
        // vel_des_lin(2) = 0.0;

        double period = 0.5;
        endEffectorOffset << 0.1881, -0.12675, -0.3,
            0.1881, 0.12675, -0.3,
            -0.1881, -0.12675, -0.3,
            -0.1881, 0.12675, -0.3;

        double step_x = vel_des[0] * period * 0.25;
        double step_y = vel_des[1] * period * 0.25;
        double omega = vel_des[2] * period * 0.25;
        double step_dy = omega * endEffectorOffset[0];

        auto shoulders = endEffectorOffset;
        for (int i = 0; i < 4; ++i)
        {
            shoulders(3 * i + 2) = 0.0;
        }

        // expected COM in world
        Eigen::Vector3d com_w;
        com_w.setZero();
        if (accurateStanceFoot_)
        {
            Eigen::Vector3d linVelPrev;
            linVelPrev.setZero();
            auto linVelTemp = (foot_pos.cast<double>() - shoulders) / (period * 0.25);
            if (phase < 0.5 * M_PI || phase > 1.5 * M_PI)
            {
                linVelPrev = (linVelTemp.segment<3>(0) + linVelTemp.segment<3>(9)) * 0.5;
            }
            else
            {
                linVelPrev = (linVelTemp.segment<3>(3) + linVelTemp.segment<3>(6)) * 0.5;
            }
            linVelPrev(2) = 0.0;
            com_w = Eigen::Vector3d(transformStamped.transform.translation.x, transformStamped.transform.translation.y, transformStamped.transform.translation.z) + rotmat * ((vel_des_lin + linVelPrev) * 0.5 * period * 0.5);
        }
        else
        {
            com_w = Eigen::Vector3d(transformStamped.transform.translation.x, transformStamped.transform.translation.y, transformStamped.transform.translation.z) + rotmat * (vel_des_lin * period * 0.5);
        }

        for (int i = 0; i < 4; ++i)
        {
            endEffectorW.segment<3>(3 * i) = Eigen::Vector3d(transformStamped.transform.translation.x, transformStamped.transform.translation.y, transformStamped.transform.translation.z) + rotmat * (foot_pos.segment<3>(3 * i).cast<double>());
        }

        // [0, 1.0 * pi): FR/HL stance
        // [1.0 * pi, 2.0 * pi): FR/HL swing
        // [0, 0.0 * pi) and [1.0, 2.0 * pi): FL/HR stance
        // [0.0, 1.0 * pi): FL/HR swing
        Eigen::Matrix<double, 4, 3> mat;
        Eigen::Matrix<double, 4, 3> mat_unchanged;
        mat.setZero();
        mat_unchanged.setZero();
        if (phase < 0.5 * M_PI || phase > 1.5 * M_PI)
        {
            // stance "backward"
            if (accurateStanceFoot_)
            {
                mat.row(0) = endEffectorW.segment<3>(0).transpose();
                mat.row(3) = endEffectorW.segment<3>(9).transpose();
            }
            else
            {
                mat.row(0) = com_w + rotmat * (shoulders.segment<3>(0) - Eigen::Vector3d(step_x, (step_y + step_dy), 0.0));
                mat.row(3) = com_w + rotmat * (shoulders.segment<3>(9) - Eigen::Vector3d(step_x, (step_y - step_dy), 0.0));
            }

            // swing "forward"
            mat.row(1) = com_w + rotmat * (shoulders.segment<3>(3) + Eigen::Vector3d(step_x, (step_y + step_dy), 0.0));
            mat.row(2) = com_w + rotmat * (shoulders.segment<3>(6) + Eigen::Vector3d(step_x, (step_y - step_dy), 0.0));
        }
        else
        {
            // stance "backward"
            if (accurateStanceFoot_)
            {
                mat.row(1) = endEffectorW.segment<3>(3).transpose();
                mat.row(2) = endEffectorW.segment<3>(6).transpose();
            }
            else
            {
                mat.row(1) = com_w + rotmat * (shoulders.segment<3>(3) - Eigen::Vector3d(step_x, (step_y + step_dy), 0.0));
                mat.row(2) = com_w + rotmat * (shoulders.segment<3>(6) - Eigen::Vector3d(step_x, (step_y - step_dy), 0.0));
            }

            // swing "forward"
            mat.row(0) = com_w + rotmat * (shoulders.segment<3>(0) + Eigen::Vector3d(step_x, (step_y + step_dy), 0.0));
            mat.row(3) = com_w + rotmat * (shoulders.segment<3>(9) + Eigen::Vector3d(step_x, (step_y - step_dy), 0.0));
        }

        ROS_DEBUG_STREAM("======================" << std::endl
                                                  << "phase: " << phase << std::endl
                                                  << "mat: " << std::endl
                                                  << mat << std::endl
                                                  << "com_w: " << com_w.transpose() << std::endl
                                                  << "shoulders: " << shoulders.transpose() << std::endl
                                                  << "endEffectorW: " << endEffectorW.transpose() << std::endl
                                                  << "foot_pos: " << foot_pos.transpose() << std::endl);

        // estimated normal
        Eigen::Matrix<double, 3, 3> rot_des;
        Eigen::Vector3d body_z_des;
        rot_des.setZero();
        body_z_des.setZero();
        double height_est = 0.0;

        for (int i = 0; i < 4; ++i)
        {
            // mat(i, 2) = map_->atPosition(elevation_layer_, grid_map::Position(mat(i, 0), mat(i, 1))) + 0.02;
            mat(i, 2) = getHeight(elevation_layer_, mat(i, 0), mat(i, 1), com_w(2) - bodyHeight_) + 0.02;
        }

        ROS_DEBUG_STREAM("stair_flag: " << stair_flag_ << std::endl);
        if (!stair_flag_)
        {
            Eigen::Vector3d plan_eqn = getPlane(mat);
            body_z_des = -plan_eqn;
            body_z_des(2) = 1.0;
            body_z_des /= body_z_des.lpNorm<2>();
            com_w(2) = 1.0;
            height_est = plan_eqn.dot(com_w);
        }
        else
        {
            // height_est = map_->atPosition(wz_layer_, grid_map::Position(com_w(0), com_w(1)));
            // body_z_des(0) = map_->atPosition(dx_layer_, grid_map::Position(com_w(0), com_w(1)));
            // body_z_des(1) = map_->atPosition(dy_layer_, grid_map::Position(com_w(0), com_w(1)));
            height_est = getHeight(wz_layer_, com_w(0), com_w(1), com_w(2) - bodyHeight_);
            body_z_des(0) = getHeight(dx_layer_, com_w(0), com_w(1), 0.0);
            body_z_des(1) = getHeight(dy_layer_, com_w(0), com_w(1), 0.0);
            if (!normalized_gradient_)
            {
                body_z_des(0) = body_z_des(0) / std::sqrt(1.0 + body_z_des(0) * body_z_des(0));
                body_z_des(1) = body_z_des(1) / std::sqrt(1.0 + body_z_des(1) * body_z_des(1));
            }
            body_z_des(2) = std::sqrt(std::max(0.0, 1.0 - body_z_des.head<2>().squaredNorm()));
        }
        ROS_DEBUG_STREAM("body_z_des 0: " << body_z_des.transpose() << std::endl);

        Eigen::Vector3d body_x = rotmat.col(0).transpose();
        // avoid large roll
        Eigen::Vector3d z_w(0.0, 0.0, 1.0);
        Eigen::Vector3d lateral_norm = z_w.cross(body_x);
        lateral_norm /= lateral_norm.lpNorm<2>();
        double roll_cos = lateral_norm.dot(body_z_des);
        if (std::fabs(roll_cos) > 0.3)
        {
            double sign = roll_cos > 0 ? 1.0 : -1.0;
            body_z_des = z_w + lateral_norm * 0.3 * sign;
            body_z_des /= body_z_des.lpNorm<2>();
        }
        // if (std::fabs(body_z_des(1) / body_z_des(2)) > 0.3)
        // {
        //     body_z_des(1) = body_z_des(1) > 0 ? body_z_des(2) * 0.3 : -body_z_des(2) * 0.3;
        //     body_z_des /= body_z_des.lpNorm<2>();
        // }
        Eigen::Vector3d body_y = body_z_des.cross(body_x);
        body_y /= body_y.lpNorm<2>();
        body_x = body_y.cross(body_z_des);
        body_x /= body_x.lpNorm<2>();
        rot_des.row(0) << body_x.transpose();
        rot_des.row(1) << body_y.transpose();
        rot_des.row(2) << body_z_des.transpose();

        com_w(2) = height_est + bodyHeight_;
        ROS_DEBUG_STREAM("body_z_des 1: " << body_z_des.transpose() << std::endl);
        ROS_DEBUG_STREAM("com_w: " << com_w.transpose() << std::endl);

        // endEffectorRefFloat << mat.row(0).transpose().cast<float>(),
        //                     mat.row(1).transpose().cast<float>(),
        //                     mat.row(2).transpose().cast<float>(),
        //                     mat.row(3).transpose().cast<float>();
        // pos_des << rot_des.col(2).cast<float>();
        // memcpy(motion_plan_lcm.ref, endEffectorRefFloat.data(), 12 * sizeof(float));
        // memcpy(motion_plan_lcm.pos, pos_des.data(), 3 * sizeof(float));
        // motion_plan_lcm.phase = phase;
        // lcm_.publish("VISION_RESPONSE_DBG_MAT_0", &motion_plan_lcm);
        ROS_DEBUG_STREAM("MAT_0: " << std::endl
                                   << mat << std::endl
                                   << "pos_des: " << pos_des.transpose() << std::endl);

        // endEffectorRefFloat << rot_des.row(0).transpose().cast<float>(),
        //                     rot_des.row(1).transpose().cast<float>(),
        //                     rot_des.row(2).transpose().cast<float>(),
        //                     0.0, 0.0, 0.0;
        // pos_des << step_x, step_y, step_dy;
        // memcpy(motion_plan_lcm.ref, endEffectorRefFloat.data(), 12 * sizeof(float));
        // memcpy(motion_plan_lcm.pos, pos_des.data(), 3 * sizeof(float));
        // motion_plan_lcm.phase = phase;
        // lcm_.publish("VISION_RESPONSE_DBG_ROT_MAT", &motion_plan_lcm);
        ROS_DEBUG_STREAM("rot_mat: " << std::endl
                                     << rot_des << std::endl);

        // 1-iter adjust for swing leg
        if (phase < 0.5 * M_PI || phase > 1.5 * M_PI)
        {
            // stance "backward"
            if (accurateStanceFoot_)
            {
                mat.row(0) = endEffectorW.segment<3>(0).transpose();
                mat.row(3) = endEffectorW.segment<3>(9).transpose();
            }
            else
            {
                mat.row(0) = com_w + rot_des.transpose() * (shoulders.segment<3>(0) - Eigen::Vector3d(step_x, (step_y + step_dy), 0.0));
                mat.row(3) = com_w + rot_des.transpose() * (shoulders.segment<3>(9) - Eigen::Vector3d(step_x, (step_y - step_dy), 0.0));
            }

            // swing "forward"
            mat.row(1) = com_w + rot_des.transpose() * (shoulders.segment<3>(3) + Eigen::Vector3d(step_x, (step_y + step_dy), 0.0));
            mat.row(2) = com_w + rot_des.transpose() * (shoulders.segment<3>(6) + Eigen::Vector3d(step_x, (step_y - step_dy), 0.0));
        }
        else
        {
            // stance "backward"
            if (accurateStanceFoot_)
            {
                mat.row(1) = endEffectorW.segment<3>(3).transpose();
                mat.row(2) = endEffectorW.segment<3>(6).transpose();
            }
            else
            {
                mat.row(1) = com_w + rot_des.transpose() * (shoulders.segment<3>(3) - Eigen::Vector3d(step_x, (step_y + step_dy), 0.0));
                mat.row(2) = com_w + rot_des.transpose() * (shoulders.segment<3>(6) - Eigen::Vector3d(step_x, (step_y - step_dy), 0.0));
            }

            // swing "forward"
            mat.row(0) = com_w + rot_des.transpose() * (shoulders.segment<3>(0) + Eigen::Vector3d(step_x, (step_y + step_dy), 0.0));
            mat.row(3) = com_w + rot_des.transpose() * (shoulders.segment<3>(9) + Eigen::Vector3d(step_x, (step_y - step_dy), 0.0));
        }

        if (ax_flag_)
        {
            ROS_DEBUG_STREAM("ax_flag_: " << ax_flag_ << std::endl
                                          << "mat: " << std::endl
                                          << mat << std::endl
                                          << "com_w: " << com_w.transpose() << std::endl);
            double ax_sum = 0.0;
            double ax_tmp = 0.0;
            if (phase < 0.5 * M_PI || phase > 1.5 * M_PI)
            {
                // 1 2 swing
                // ax_tmp = map_->atPosition(ax_layer_, grid_map::Position(mat(1, 0), mat(1, 1)));
                ax_tmp = getHeight(ax_layer_, mat(1, 0), mat(1, 1), 0.0);
                mat(1, 0) += ax_tmp;
                ax_sum += ax_tmp;

                // ax_tmp = map_->atPosition(ax_layer_, grid_map::Position(mat(2, 0), mat(2, 1)));
                ax_tmp = getHeight(ax_layer_, mat(2, 0), mat(2, 1), 0.0);
                mat(2, 0) += ax_tmp;
                ax_sum += ax_tmp;
            }
            else
            {
                // 0 3 swing
                // ax_tmp = map_->atPosition(ax_layer_, grid_map::Position(mat(0, 0), mat(0, 1)));
                ax_tmp = getHeight(ax_layer_, mat(0, 0), mat(0, 1), 0.0);
                mat(0, 0) += ax_tmp;
                ax_sum += ax_tmp;

                // ax_tmp = map_->atPosition(ax_layer_, grid_map::Position(mat(3, 0), mat(3, 1)));
                ax_tmp = getHeight(ax_layer_, mat(3, 0), mat(3, 1), 0.0);
                mat(3, 0) += ax_tmp;
                ax_sum += ax_tmp;
            }
            com_w(0) += ax_sum / 4;
            // height_est = map_->atPosition(wz_layer_, grid_map::Position(com_w(0), com_w(1)));
            height_est = getHeight(wz_layer_, com_w(0), com_w(1), com_w(2) - bodyHeight_);
            com_w(2) = height_est + bodyHeight_;
            ROS_DEBUG_STREAM("mat: " << std::endl
                                     << mat << std::endl
                                     << "com_w: " << com_w.transpose() << std::endl);
        }

        // avoid edge
        if (sample_based_adjustment_)
        {
            // backup old pose (x, y)
            mat_unchanged = mat;

            // adjust for new pose
            double step_size = 0.05;
            double threshold = 2.0;
            std::array<double, 4> adj_size = {0.035, 0.065, -0.075, -0.025};
            Eigen::Vector2d gradient;
            Eigen::VectorXd heights;
            grid_map::Position pos;
            Eigen::Vector2d d_com_w;
            gradient.setZero();
            heights.setZero(5);
            pos.setZero();
            d_com_w.setZero();
            for (int i = 0; i < 4; ++i)
            {
                if (phase < 0.5 * M_PI || phase > 1.5 * M_PI)
                {
                    // 1 2 swing
                    if (i == 0 || i == 3)
                    {
                        continue;
                    }
                }
                else
                {
                    // 0 3 swing
                    if (i == 1 || i == 2)
                    {
                        continue;
                    }
                }

                // gradient(0) = map_->atPosition(dx_layer_, grid_map::Position(mat(i, 0), mat(i, 1)));
                // gradient(1) = map_->atPosition(dy_layer_, grid_map::Position(mat(i, 0), mat(i, 1)));
                gradient(0) = getHeight(dx_layer_, mat(i, 0), mat(i, 1), 0.0);
                gradient(1) = getHeight(dy_layer_, mat(i, 0), mat(i, 1), 0.0);
                if (gradient.lpNorm<2>() < 0.12)
                {
                    continue;
                }
                gradient /= gradient.lpNorm<2>();
                gradient *= -1.0;

                pos << mat(i, 0), mat(i, 1);
                pos -= gradient * step_size;
                // heights(0) = map_->atPosition(elevation_layer_, pos);
                heights(0) = getHeight(elevation_layer_, pos(0), pos(1), mat(i, 2));
                for (int j = 0; j < 4; ++j)
                {
                    pos += gradient * step_size;
                    // heights(j + 1) = map_->atPosition(elevation_layer_, pos);
                    heights(j + 1) = getHeight(elevation_layer_, pos(0), pos(1), mat(i, 2));
                }
                auto dh = (heights.tail<4>() - heights.head<4>()).cwiseAbs() / step_size;
                for (int j = 0; j < 4; ++j)
                {
                    if (dh(j) > threshold)
                    {
                        // mat.row(i).head<2>() = mat.row(i).head<2>() + gradient.transpose() * adj_size.at(j);
                        mat(i, 0) += gradient(0) * adj_size.at(j);
                        mat(i, 1) += gradient(1) * adj_size.at(j);
                        d_com_w += gradient * adj_size.at(j);
                        break;
                    }
                }
            }
            com_w.head<2>() += d_com_w / 4.0;
            // height_est = map_->atPosition(wz_layer_, grid_map::Position(com_w(0), com_w(1)));
            height_est = getHeight(wz_layer_, com_w(0), com_w(1), com_w(2) - bodyHeight_);
            com_w(2) = height_est + bodyHeight_;
        }

        for (int i = 0; i < 4; ++i)
        {
            // mat(i, 2) = map_->atPosition(elevation_layer_, grid_map::Position(mat(i, 0), mat(i, 1))) + 0.02;
            mat(i, 2) = getHeight(elevation_layer_, mat(i, 0), mat(i, 1), com_w(2) - bodyHeight_) + 0.02;
        }
        // TBD: process nan values

        // publish log
        auto log_msg = stair_ctrl::MotionPlan();
        for (size_t i = 0; i < 4; ++i)
        {
            for (size_t j = 0; j < 3; ++j)
            {
                log_msg.foot_pos.at(3 * i + j) = mat(i, j);
            }
        }
        for (size_t i = 0; i < 3; ++i)
        {
            log_msg.vec3.at(i) = com_w(i);
        }
        log_msg.phase = phase;
        log_pub_.publish(log_msg);

        // calculate feet position from world to body
        Eigen::Matrix<double, 12, 1> endEffectorRef;
        endEffectorRef.setZero();
        for (int i = 0; i < 4; ++i)
        {
            endEffectorRef.segment<3>(3 * i) = rot_des * (mat.row(i).transpose() - com_w);
        }
        pos_des << rot_des.col(2).cast<float>();
        endEffectorRefFloat << endEffectorRef.cast<float>();

        // memcpy(motion_plan_lcm.ref, endEffectorRefFloat.data(), 12 * sizeof(float));
        // memcpy(motion_plan_lcm.pos, pos_des.data(), 3 * sizeof(float));
        // motion_plan_lcm.phase = phase;
        // lcm_.publish("VISION_RESPONSE", &motion_plan_lcm);
        auto motion_plan = stair_ctrl::MotionPlan();
        for (size_t i = 0; i < 12; ++i)
        {
            motion_plan.foot_pos.at(i) = endEffectorRefFloat(i);
        }
        for (size_t i = 0; i < 3; ++i)
        {
            motion_plan.vec3.at(i) = pos_des(i);
        }
        motion_plan.phase = phase;
        plan_pub_.publish(motion_plan);
        ROS_DEBUG_STREAM("MAT_1: " << std::endl
                                   << mat << std::endl
                                   << "pos_des: " << pos_des.transpose() << std::endl);
        ROS_DEBUG_STREAM("motion_plan.foot_pos: " << std::endl
                                                  << endEffectorRefFloat.segment<3>(0).transpose() << std::endl
                                                  << endEffectorRefFloat.segment<3>(3).transpose() << std::endl
                                                  << endEffectorRefFloat.segment<3>(6).transpose() << std::endl
                                                  << endEffectorRefFloat.segment<3>(9).transpose() << std::endl
                                                  << "motion_plan.vec3: " << pos_des.transpose());

        // endEffectorRefFloat << mat.row(0).transpose().cast<float>(),
        //                     mat.row(1).transpose().cast<float>(),
        //                     mat.row(2).transpose().cast<float>(),
        //                     mat.row(3).transpose().cast<float>();
        // pos_des << com_w.cast<float>();
        // memcpy(motion_plan_lcm.ref, endEffectorRefFloat.data(), 12 * sizeof(float));
        // memcpy(motion_plan_lcm.pos, pos_des.data(), 3 * sizeof(float));
        // motion_plan_lcm.phase = phase;
        // lcm_.publish("VISION_RESPONSE_DBG_MAT_1", &motion_plan_lcm);

        // endEffectorRefFloat = endEffectorW.cast<float>();
        // pos_des << com_w.cast<float>();
        // memcpy(motion_plan_lcm.ref, endEffectorRefFloat.data(), 12 * sizeof(float));
        // memcpy(motion_plan_lcm.pos, pos_des.data(), 3 * sizeof(float));
        // motion_plan_lcm.phase = phase;
        // lcm_.publish("VISION_RESPONSE_DBG_MAT_2", &motion_plan_lcm);

        // geometry_msgs::PoseArray poses;
        // poses.header.stamp = ros::Time::now();
        // poses.header.frame_id = map_frame_;
        // for (int i = 0; i < 4; ++i)
        // {
        //     geometry_msgs::Pose p;
        //     p.position.x = endEffectorW(3 * i);
        //     p.position.y = endEffectorW(3 * i + 1);
        //     p.position.z = endEffectorW(3 * i + 2);
        //     poses.poses.push_back(p);
        // }
        // for (int i = 0; i < 4; ++i)
        // {
        //     geometry_msgs::Pose p;
        //     p.position.x = mat(i, 0);
        //     p.position.y = mat(i, 1);
        //     p.position.z = mat(i, 2) - 0.02;
        //     poses.poses.push_back(p);
        // }
        // if (sample_based_adjustment_)
        // {
        //     for (int i = 0; i < 4; ++i)
        //     {
        //         geometry_msgs::Pose p;
        //         p.position.x = mat_unchanged(i, 0);
        //         p.position.y = mat_unchanged(i, 1);
        //         p.position.z = 0.0;
        //         poses.poses.push_back(p);
        //     }
        // }
        // plan_pub_.publish(poses);
    }

    inline double getHeight(const std::string& layer, double x, double y, double default_value)
    {
        auto z = map_->atPosition(layer, grid_map::Position(x, y));
        // ROS_DEBUG_STREAM("[NAN] (" << x << ", " << y << "): " << std::isnan(z) << std::endl);
        if (std::isnan(z))
        {
            ROS_DEBUG_STREAM("[NAN] get NaN at: (" << x << ", " << y << ") !!!"
                            << " Replace z by: " << default_value << std::endl);
            z = default_value;
        }
        return z;
    }
    
    void start()
    {
        ros::AsyncSpinner map_spinner(1, &map_queue_);
        map_spinner.start();
        ros::spin();
    }
};